package ec.cd.client;

import java.util.HashMap;
import java.util.LinkedList;
import java.util.concurrent.ArrayBlockingQueue;
import java.util.Queue;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

import ec.cd.common.Sensors;
import ec.cd.common.Sensors.SensorRunningMode;
import ec.cd.common.Sensors.SensorConfig;
import ec.cd.common.Sensors.SensorData;

public class SensorsManager implements SensorEventListener {
    private static String TAG = "SensorsManager";

    public SensorsManager(Context context) {
        sensorManager =
                (SensorManager) context.getSystemService(Context.SENSOR_SERVICE);
        gyroscope =
                sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
        accelerometer =
                sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);

        sensorsConfig = new HashMap<Integer, SensorConfig>();
        sensorsDataRange = new HashMap<Integer, Integer>();
        sensorsData = new ArrayBlockingQueue<SensorData>(1000);
    }

    protected void startListening() {
        if (sensorsConfig.get(Sensors.ST_GYROSCOPE) != null) {
            sensorManager.registerListener(this, gyroscope,
                    sensorsConfig.get(Sensors.ST_GYROSCOPE).sensorSamplingRate);
        }
        if (sensorsConfig.get(Sensors.ST_ACCELEROMETER) != null) {
            sensorManager.registerListener(this, accelerometer,
                    sensorsConfig.get(Sensors.ST_ACCELEROMETER).sensorSamplingRate);
        }
    }

    protected void stopListening() {
        if (sensorsConfig.get(Sensors.ST_ACCELEROMETER) != null) {
            sensorManager.unregisterListener(this, accelerometer);
        }
        if (sensorsConfig.get(Sensors.ST_GYROSCOPE) != null) {
            sensorManager.unregisterListener(this, gyroscope);
        }
    }

    // srm: Sensor running mode
    // scs: SensorConfig's, can be null if srm = Calibration
    // Disable all sensors and enable sensors specified in the array
    // Undefined behaviour if scs is invalid
    public void enableSensors(SensorRunningMode srm, SensorConfig[] scs) {
        disableAllSensors();

        sensorRunningMode = srm;
        switch (sensorRunningMode) {
            case Calibration:
                SensorConfig gyro = new SensorConfig();
                gyro.sensorType = Sensors.ST_GYROSCOPE;
                gyro.sensorSamplingRate = Sensors.SSR_FASTEST;
                sensorsConfig.put(gyro.sensorType, gyro);
                sensorsDataRange.put(gyro.sensorType, -1);

                SensorConfig acc = new SensorConfig();
                acc.sensorType = Sensors.ST_ACCELEROMETER;
                acc.sensorSamplingRate = Sensors.SSR_FASTEST;
                sensorsConfig.put(acc.sensorType, acc);
                sensorsDataRange.put(acc.sensorType, -1);
                break;
            case Normal:
            case PowerSave:
                for (SensorConfig sc : scs) {
                    SensorConfig new_sc = new SensorConfig();
                    new_sc.sensorType = sc.sensorType;
                    new_sc.sensorSamplingRate = sc.sensorSamplingRate;
                    new_sc.ranges = sc.ranges.clone();
                    sensorsConfig.put(new_sc.sensorType, new_sc);
                    sensorsDataRange.put(new_sc.sensorType, -1);
                }
        }

        startListening();
    }

    public void disableAllSensors() {
        stopListening();
        sensorsDataRange.clear();
        sensorsConfig.clear();
    }

    public void onAccuracyChanged(Sensor sensor, int accuracy) {
    }

    public void onSensorChanged(SensorEvent event) {
        float[] vals = event.values;
        int st = event.sensor.getType();

        switch (sensorRunningMode) {
            case Calibration:
                break;
            case Normal:
                if (!validSensorData3(sensorsConfig.get(st).ranges, vals))
                    return;
                break;
            case PowerSave:
                if (st == Sensor.TYPE_ACCELEROMETER)
                    vals = Sensors.calcAccTilt(event.values); // To tilt

                int new_sensor_range =
                    mapSensorRange(sensorsConfig.get(st).ranges, vals);
                if (sensorsDataRange.get(st) == new_sensor_range)
                    return;
                else sensorsDataRange.put(st, new_sensor_range);
                break;
        }

        SensorData data = new SensorData();
        data.sensorType = st;
        data.timestamp = event.timestamp;
        data.values = vals.clone();
        sensorsData.offer(data);
    }

    // Sensor data accumulated until now
    // Caller should remove any used data
    public Queue<SensorData> getSensorData() {
        return sensorsData;
    }

    private static boolean isWithin(float val, float min, float max) {
        return val < min && min < max;
    }

    private static boolean validSensorData3(float[] ranges, float[] vals) {
        return isWithin(vals[0], ranges[0], ranges[1]) &&
               isWithin(vals[1], ranges[0], ranges[1]) &&
               isWithin(vals[2], ranges[0], ranges[1]);
    }

    // If the ranges in each axis disagree with each other, return -1
    private static int mapSensorRange(float[] ranges, float[] vals) {
        int[] range_results = new int[3];
        for (int i = 0; i != range_results.length; ++i) {
            range_results[i] = -1;
            for (int j = 0; j != ranges.length/2; ++j)
                if (isWithin(vals[i], ranges[j], ranges[j+1])) {
                    range_results[i] = j;
                    break;
                }
        }

        for (int i = 1; i != 3; ++i)
            if (range_results[0] != range_results[i])
                return -1;

        return range_results[0];
    }

    private SensorManager sensorManager;
    private Sensor gyroscope;
    private Sensor accelerometer;

    private SensorRunningMode sensorRunningMode;
    private HashMap<Integer, SensorConfig> sensorsConfig;
    // This shows what range the last sensor data is in, -1 for out of range
    // Used in PowerSave mode only
    private HashMap<Integer, Integer> sensorsDataRange;
    private Queue<SensorData> sensorsData;
}
